#ifndef __BACKTRACKING_H_
#define __BACKTRACKING_H_

#include <boost/shared_array.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <vector>
#include <deque>

template <typename VALUE>
class Backtracking
{
public:
	typedef typename mtl::dense2D<VALUE>					MATRIX;
	typedef typename mtl::Collection<MATRIX>::value_type	SCALAR;
	typedef typename mtl::dense_vector<VALUE>				VECTOR;
	typedef typename Mesh::Point							POINT; 
	typedef typename Mesh::FaceHandle                       FACEHANDLE;
	typedef typename Mesh::VertexHandle                     VERTEXHANDLE; 
	typedef typename boost::shared_array<VALUE>				ARRAY;

	struct SEGMENT
	{
	public:
		POINT begin;
		POINT end;
	};

public:
	static bool LU_Solver (const MATRIX & A, const VECTOR & b, VECTOR & solution)
	{
		try
		{
			solution = lu_solve(A,b);
			VECTOR verify (A * solution);
		}
		catch (mtl::matrix_singular e)
		{
			//The matrix is singular
			return false;
		}
		return true;
	}
	//Extropolate from point x_i to point x_j
	static POINT Huen (const VECTOR coeff, const POINT & x_i, const VALUE step)
	{
		VALUE * pco = new VALUE[size(coeff)];
		ARRAY co (pco); 
		for (int i = 0;i < size(coeff); ++i) co [i] = coeff[i];
		POINT pres = Huen (co, x_i, step);
		return pres; 
	}
	static POINT Huen (ARRAY coeff, const POINT & x_i, const VALUE step)
	{
		POINT x_inter;
		x_inter [0] = x_i[0] - step * gradx(coeff, x_i[0], x_i[1]) ;
		x_inter [1] = x_i[1] - step * grady(coeff, x_i[0], x_i[1]) ;
		x_inter [2] = 0;

		boost::shared_ptr<POINT> x_j ( new POINT );
		(*x_j) [0] = x_i[0] - step / VALUE(2.0) * (gradx(coeff, x_i[0], x_i[1]) + gradx(coeff, x_inter[0], x_inter[1]));
		(*x_j) [1] = x_i[1] - step / VALUE(2.0) * (grady(coeff, x_i[0], x_i[1]) + grady(coeff, x_inter[0], x_inter[1]));
		(*x_j) [2] = 0;
		return (*x_j);
	}

public: 
	static bool intersection(SEGMENT & seg1, SEGMENT & seg2, POINT & p)
	{
		POINT point[4];
		point[0] = seg1.begin; point[1] = seg1.end;
		point[2] = seg2.begin; point[3] = seg2.end;
		if (fabs(point [0][1] - point [1][1]) < ERROR)
		{
			double k2 = (point[2][0] - point[3][0])/ (point[2][1] - point[3][1]);
			p[1] = point[0][1];
			p[0] = point[2][0] + k2 * (p[1] - point[2][1]);
			p[2] = 0;
		}
		else
		if (fabs(point[2][1] - point[3][1]) < ERROR)
		{
			double k1 = (point[0][0] - point[1][0])/ (point[0][1] - point[1][1]);
			p[1] = point[2][1];
			p[0] = point[0][0] + k1 * (p[1] - point[0][1]);
			p[2] = 0;
		}
		else
		{
			double k1 = (point[0][0] - point[1][0])/ (point[0][1] - point[1][1]);
			double k2 = (point[2][0] - point[3][0])/ (point[2][1] - point[3][1]);
			double x = ((point[2][0] - k2 * point[2][1]) * (k1) - (point[0][0] - k1 * point[0][1]) * (k2)) /( k1 - k2);
			double y = ((point[2][0] - k2 * point[2][1]) - (point[0][0] - k1 * point[0][1]) ) /( k1 - k2);
			p[0] = x;
			p[1] = y;
			p[2] = 0;
		}
		//std::cerr<<"Intersection Point "<<x<<' '<<y<<std::endl;

		if ( (p[0] - point[0][0])  * (point[1][0] - p[0]) >= 0 && (p[0] - point[2][0])*(point[3][0] - p[0])>=0)
		{
			if ((p[1] - point[0][1])  * (point[1][1] - p[1]) >= 0 && (p[1] - point[2][1])*(point[3][1] - p[1])>=0)
			{
				return true;
			}
			return false;
		}
		return false;
		/*
		//Check whether the Intersection is on the segment. 
		if ( x <= point[2][0] && x >= point[3][0] && y <= point[2][1] && y >= point[3][1]
		||   x >= point[2][0] && x <= point[3][0] && y <= point[2][1] && y >= point[3][1]
		||	 x <= point[2][0] && x >= point[3][0] && y >= point[2][1] && y <= point[3][1]
		||   x >= point[2][0] && x <= point[3][0] && y >= point[2][1] && y <= point[3][1])
		{
			return true;
		}
		else
		{
			__DEBUG_MSG("Intersection Error");
			return false;
		}
		*/
		return true;
	}
	static void map_threepoint_to_plane(const POINT point [3], POINT result [3], MATRIX & rotation, VECTOR & translation)
	{
		Mesh::Point A = point[0];
		Mesh::Point B = point[1];
		Mesh::Point C = point[2];


		double AB = (B-A).length();
		double AC = (C-A).length();
		double BC = (C-B).length();
		//Firstly, we flip the triangle ABC on the x-y plane
		result[0] = Mesh::Point(0,0,0);
		result[1] = Mesh::Point( AB, 0, 0);

		double theta = acos((AB* AB + AC*AC - BC*BC)/ (2.0*AB*AC));
		result[2] = Mesh::Point (AC * cos(theta) , AC * sin(theta), 0);
        //Compute the rotation and translation ;
		POINT v[3], flip_v[3];
		for (int i = 0; i < 3; ++i)
		{
			v[i] = point[i];
			flip_v[i] = result[i];
		}
		convert_coordinate(v, flip_v, rotation, translation);
	}
	static void map_sixpoint_to_plane(const POINT point [6], POINT result [6], MATRIX & rotation, VECTOR & translation)
	{
		Mesh::Point A = point[0];
		Mesh::Point B = point[1];
		Mesh::Point C = point[2];
		Mesh::Point D = point[3];
		Mesh::Point E = point[4];
		Mesh::Point F = point[5];

		double AB = (B-A).length();
		double AC = (C-A).length();
		double BC = (C-B).length();

		double AD = (D-A).length();
		double BD = (D-B).length();

		double AF = (F-A).length();
		double CF = (F-C).length();

		double CE = (E-C).length();
		double BE = (E-B).length();

		//Firstly, we flip the triangle ABC on the x-y plane
		result[0] = Mesh::Point(0,0,0);
		result[1] = Mesh::Point( AB, 0, 0);

		double theta = acos((AB* AB + AC*AC - BC*BC)/ (2.0*AB*AC));
		result[2] = Mesh::Point (AC * cos(theta) , AC * sin(theta), 0);

		//Secondly, we flip the points DEF to the x-y plane
		double alpha = acos((AB* AB + AD*AD - BD*BD) / (2.0 * AB * AD));
		result[3] = Mesh::Point (AD * cos(alpha), AD * sin(alpha) * (-1.0),0 );

		double mu	 = acos((BC*BC + BE * BE - CE * CE) / (2.0 * BC * BE)) ;
		double delta = acos((AB * AB + BC * BC - AC * AC) / (2.0 * AB * BC)) ;

		result[4] = Mesh::Point (AB + BE * cos (PI - mu - delta), BE * sin (PI - mu - delta), 0);

		double beta = acos((AF* AF+ AC * AC - CF * CF )/ (2.0 * AF * AC));
		result[5] = Mesh::Point (AF * cos(theta + beta) , AF * sin(theta + beta), 0);

		//Compute the rotation and translation ;
		POINT v[3], flip_v[3];
		for (int i = 0; i < 3; ++i)
		{
			v[i] = point[i];
			flip_v[i] = result[i];
		}
		convert_coordinate(v, flip_v, rotation, translation);

	}

	// TriA is the original triangles. TriB is the flipped one. 
	// Find a rotation matrix and a translation vector to map the point from TriA to TriB
	// R (A + t) = B
	static void convert_coordinate(const POINT A[3], const POINT tri_B[3], MATRIX & rotation, VECTOR & translation)
	{
		POINT tri_A[3];
		tri_A[0] = A[0]; tri_A[1] = A[1]; tri_A[2] = A[2];
		VECTOR trans(3);
		for (int i = 0;i < 3; ++i) trans[i] = tri_B[0][i] - tri_A[0][i]; 
		translation = trans;
		for (int i = 0; i < 3; ++i)
		{
			tri_A[0][i] += trans[i]; 
			tri_A[1][i] += trans[i]; 
			tri_A[2][i] += trans[i];
		}

		Mesh::Point alpha[3];
		alpha[0] = tri_A[1] ; alpha[1] = tri_A[2]; 
		alpha[2][0] = tri_A[1][1] * tri_A[2][2] - tri_A[2][1] * tri_A[1][2];
		alpha[2][1] = tri_A[2][0] * tri_A[1][2] - tri_A[1][0] * tri_A[2][2];
		alpha[2][2] = tri_A[1][0] * tri_A[2][1] - tri_A[2][0] * tri_A[1][1];
		//for (int i = 0; i < 3; ++i) alpha[2][i] += trans[i]; 


		Mesh::Point beta[3];
		beta[0] = tri_B[1] ; beta[1] = tri_B[2]; 
		beta[2][0] = tri_B[1][1] * tri_B[2][2] - tri_B[2][1] * tri_B[1][2];
		beta[2][1] = tri_B[2][0] * tri_B[1][2] - tri_B[1][0] * tri_B[2][2];
		beta[2][2] = tri_B[1][0] * tri_B[2][1] - tri_B[2][0] * tri_B[1][1];
		//for (int i = 0; i < 3; ++i) beta[2][i] += trans[i]; 


		MATRIX M_A (3,3), M_B (3,3);

		for (int i = 0;i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				M_A[i][j] = alpha[j][i];
				M_B[i][j] = beta[j][i];
			}
		}
		rotation =  M_B * inv(M_A);
	}
	//Convert from the model to the flip one
	static void convert_A2B(const POINT & A, MATRIX & rotation, VECTOR & trans, POINT & B)
	{
		VECTOR v(3), res(3);
		v = point2vector(A);
		v = v + trans; 
		res = rotation * v ;
		B = vector2point(res);
	}
	//convert from the flip one to the model
	//rotation and translation are still computed from the convert_coordinate. 
	static void convert_B2A(const POINT & B, MATRIX & rotation, VECTOR & trans, POINT & A)
	{
		MATRIX invr = inv(rotation);
		VECTOR invt ;
		invt = -1 * rotation * trans;
		convert_A2B (B, invr, invt,  A); 
	}
	static bool three_points_interpolation (const POINT & begin, POINT vertex[3], VALUE value [3], double step, POINT & end,int & next_face_idx)
	{
		POINT flip_v[3];
		MATRIX rotation;
		VECTOR translation;
		map_threepoint_to_plane (vertex, flip_v, rotation, translation);

////////////////////////////////////
		//Interpolate the size points
		VECTOR coeff;
		interpolation3 (flip_v, value, coeff);

		POINT x_0;
		convert_A2B(begin, rotation, translation, x_0);

		POINT x_new, x_0_old;
		x_0_old = x_0;
		
		
		int edge_idx; 
		for (int i = 0 ;  ; ++i)
		{
		   
			//x_new = Huen(coeff, x_0, step);
			x_new[0] = coeff[1] * step;
			x_new[1] = coeff[2] * step;
			//	std::cout<<"x_0   "<<x_0<<std::endl;
			// std::cout<<"x_new "<<x_new<<std::endl;
			if (!is_in_triangle (flip_v, x_new, edge_idx)) break;
			//if (!is_in_face (flip_v, x_new, edge_idx)) break;
			//FIXME HERE
			if ((x_0-x_new).length() < 1e-8) 
			{
				std::cerr<<"FIXME HERE"<<std::endl;
				break;
			}
			x_0 = x_new; 
		}

		for (edge_idx = 0; edge_idx < 3; ++edge_idx)
		{
			SEGMENT seg1, seg2;
			seg1.begin = flip_v[edge_idx];
			seg1.end = flip_v[(1+edge_idx)%3];

			seg2.begin = x_0;
			seg2.end = x_new;

		
			POINT inp;
			if (intersection(seg1,seg2, inp))
			{
				next_face_idx = edge_idx; 
				convert_B2A (inp, rotation, translation, end);
				return true;
			}
			//std::cout<<edge_idx<<std::endl;
		}
		return false;
		
		
	}

	static bool six_points_interpolation (const POINT & begin, POINT vertex[], VALUE value [], double step, POINT & end,int & next_face_idx)
	{
		POINT flip_v[6];
		MATRIX rotation;
		VECTOR translation; 
		map_sixpoint_to_plane(vertex, flip_v, rotation, translation);
		
		/*
		std::cout<<"Triangle"<<std::endl;
		std::cout<<flip_v[0]<<std::endl;
		std::cout<<flip_v[1]<<std::endl;
		std::cout<<flip_v[2]<<std::endl;
		std::cout<<flip_v[3]<<std::endl;
		std::cout<<flip_v[4]<<std::endl;
		std::cout<<flip_v[5]<<std::endl;

		
		for (int i = 0; i < 6; ++i)
		{
			std::cout<<value[i]<<std::endl;
		}
		*/
		//Interpolate the size points
		VECTOR coeff;
		interpolation (flip_v, value, coeff);

		POINT x_0;
		convert_A2B(begin, rotation, translation, x_0);

		POINT x_new, x_0_old;
		x_0_old = x_0;
		
		
		int edge_idx; 
		for (int i = 0 ;  ; ++i)
		{
			x_new = Huen(coeff, x_0, step);
			//std::cout<<"x_0   "<<x_0<<std::endl;
			//std::cout<<"x_new "<<x_new<<std::endl;
			if (!is_in_triangle (flip_v, x_new, edge_idx)) break;
			//if (!is_in_face (flip_v, x_new, edge_idx)) break;
			//FIXME HERE
			if ((x_0-x_new).length() < 1e-8) 
			{
				std::cerr<<"FIXME HERE singular point"<<std::endl;
				break;
			}
			x_0 = x_new; 
		}

		for (edge_idx = 0; edge_idx < 3; ++edge_idx)
		{
			SEGMENT seg1, seg2;
			seg1.begin = flip_v[edge_idx];
			seg1.end = flip_v[(1+edge_idx)%3];

			seg2.begin = x_0;
			seg2.end = x_new;

		
			POINT inp;
			if (intersection(seg1,seg2, inp))
			{
				next_face_idx = edge_idx; 
				convert_B2A (inp, rotation, translation, end);
				return true;
			}
			//std::cout<<edge_idx<<std::endl;
		}
		return false;
	}

	static bool six_points_interpolation_test_face (Mesh & mesh, const POINT & point, FACEHANDLE face_handle, double step)
	{
		POINT vertex[6];
		VALUE value [6];
		VERTEXHANDLE vh[6];
		Mesh::FaceHalfedgeIter fh_it;
		int count = 0; 
		for (fh_it = mesh.fh_iter(face_handle); fh_it; ++fh_it, ++ count)
		{
			vh[count] = mesh.to_vertex_handle(fh_it);
			vertex[count] = mesh.point(vh[count]);
			value[count] = mesh.data(vh[count]).get_distance();
			
			vh[count + 3] =  mesh.to_vertex_handle
				(mesh.next_halfedge_handle(mesh.opposite_halfedge_handle(mesh.next_halfedge_handle(fh_it))));
			vertex[count + 3] = mesh.point(vh[count+3]);
			value[count+3] = mesh.data(vh[count+3]).get_distance();
	    }
		POINT flip_v[6];
		MATRIX rotation;
		VECTOR translation; 
		map_sixpoint_to_plane(vertex, flip_v, rotation, translation);

		VECTOR coeff;
		interpolation (flip_v, value, coeff);

		POINT x_0;
		convert_A2B(point, rotation, translation, x_0);

		POINT x_new, x_0_old;
		
		x_new = Huen(coeff, x_0, step);
		//std::cout<<"x_0   "<<x_0<<std::endl;
		//std::cout<<"x_new "<<x_new<<std::endl;
		int edge_idx; 
		if (!is_in_triangle (flip_v, x_new, edge_idx)) return false;
		else return true;
		//if (!is_in_face (flip_v, x_new, edge_idx)) break;
		//FIXME HERE
		return false;
	}

	//For each vertex point, we have two different condition. One is the vertex should go follow the edge. The other is going follow one face. 
	static bool vertex_point_next_face(Mesh & mesh , const VERTEXHANDLE vertex_handle, FACEHANDLE & face_handle, VALUE step)
	{
//		int count = 0; 
		for (Mesh::VertexFaceIter vf_it = mesh.vf_iter(vertex_handle); vf_it; ++vf_it)
		{
			//Test which face can the point go through.
			if (six_points_interpolation_test_face (mesh, mesh.point(vertex_handle), vf_it.handle(), step ))
			{
				face_handle = vf_it.handle();
				//count ++;
				break;
			}
		}
//		std::cout<<"Count "<<count<<std::endl;
	}
	static bool face_point_next_point (Mesh & mesh, const POINT & curr_point, const FACEHANDLE curr_face, double step, POINT & next_point, FACEHANDLE & next_face )
	{
		Mesh::FaceHalfedgeIter fh_it;
		Mesh::VertexHandle vh[6];
		Mesh::Point vp [6];
		double value [6];
		int count = 0;
		for (fh_it = mesh.fh_iter(curr_face); fh_it; ++fh_it, ++ count)
		{
			vh[count] = mesh.to_vertex_handle(fh_it);
			vp[count] = mesh.point(vh[count]);
			value[count] = mesh.data(vh[count]).get_distance();
			
			vh[count + 3] =  mesh.to_vertex_handle
				(mesh.next_halfedge_handle(mesh.opposite_halfedge_handle(mesh.next_halfedge_handle(fh_it))));
			vp[count + 3] = mesh.point(vh[count+3]);
			value[count+3] = mesh.data(vh[count+3]).get_distance();
	    }
		Mesh::Point intsec;
		int next_face_count;
		
		//if (!three_points_interpolation(curr_point, vp, value, step, next_point , next_face_count))

		if (!six_points_interpolation(curr_point, vp, value, step, next_point , next_face_count))
		{
			return false;
		}

		
		//Find next Face
		//face_record.push_back(curr_face);
		mesh.data(curr_face).across_count = 1;

		//Find next Face
		fh_it = mesh.fh_iter (curr_face);
		for (int j = 0; j < next_face_count + 1; ++j) ++ fh_it; 
		Mesh::HalfedgeHandle next_point_heh = fh_it.handle();
		Mesh::HalfedgeHandle adj_heh = mesh.opposite_halfedge_handle(next_point_heh); 
		next_face = mesh.face_handle(adj_heh);

		if (mesh.data(next_face).across_count )
		{
			std::cerr<<"FIXME HERE, cross face"<<std::endl;
			//Modify the next_point to be one of the end POINT
			VERTEXHANDLE to_vertex_handle = mesh.to_vertex_handle (next_point_heh);
			VERTEXHANDLE from_vertex_handle = mesh.from_vertex_handle (next_point_heh);
			VERTEXHANDLE next_vertex_handle; 
			if ((next_point - mesh.point(to_vertex_handle)).length() < (next_point - mesh.point(from_vertex_handle)).length())
			{
				next_vertex_handle = to_vertex_handle;
			}
			else
			{
				next_vertex_handle = from_vertex_handle; 
			}
			/*
			if (mesh.data(to_vertex_handle).get_distance() < mesh.data(from_vertex_handle).get_distance())
			{
				next_vertex_handle = to_vertex_handle; 
			}
			else
			{
				next_vertex_handle = from_vertex_handle;
			}
			*/
			next_point = mesh.point(next_vertex_handle);
			vertex_point_next_face(mesh , next_vertex_handle, next_face, 0.001);
			//Find the next face 
			return true;
		}
	    else
		{
			return true; 
		}
	}

	//Why we need to define the end point face handle is a set? Because the endpoint can be a vertex or a point on the edge, which we cannot assume the face it belongs to.
	//But if the pertubation is invited. I think we only need to assigned one face handle.
    static void draw_geodesics(Mesh & mesh, const POINT & start,  const FACEHANDLE start_face_handle, const POINT & end, const std::vector<FACEHANDLE> &  vec_end_face_handle, std::list<POINT> & path, std::vector<FACEHANDLE> & face_record, double step)
	{

		if (path.size())
		{
			std::cerr<<"The inital path is not empty"<<std::endl;
			return;
		}

		path.push_back(start);
		face_record.push_back(start_face_handle);

		POINT current = start;
		FACEHANDLE current_face_handle = start_face_handle;
		POINT next_point;
		FACEHANDLE next_face_handle;

//		std::vector<FACEHANDLE> face_record;
		while (! is_reach_end (vec_end_face_handle, current_face_handle))
		{
			if (false == face_point_next_point (mesh, current, current_face_handle, step, next_point, next_face_handle)) break;
			current_face_handle = next_face_handle;
			current = next_point;
			path.push_back(current);
			face_record.push_back(next_face_handle);
		}
		path.push_back(end);
		
		//Clear the face record for this path
		//FIXME HERE
		for (int i = 0; i < face_record.size(); ++i)
		{
			mesh.data(face_record[i]).across_count = 0;
		}
//		face_record.clear();

	}
public:
	static bool is_in_face (Mesh & mesh, FACEHANDLE  face_handle, POINT & p)
	{
		POINT vertex[3];
		int i = 0;
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(face_handle); fv_it; ++fv_it)
		{
			vertex[i] = mesh.point (fv_it);
			i ++;
		}		
		int edge_idx;

		return is_in_face(vertex, p, edge_idx);
	}
	static bool is_reach_end (const std::vector <FACEHANDLE> & vec_end_face_handle, FACEHANDLE current)
	{
		for (int i = 0; i < vec_end_face_handle.size(); ++i)
		{
			if (vec_end_face_handle[i] == current) return true;
		}
		return false; 
	}
	static bool is_in_face (const POINT vertex[],const POINT & pt, int & edge_idx)
	{

		POINT tri[3];
		for (int i = 0; i < 3; ++i)
		{
			tri[i] = vertex[i];
		}

		POINT p = pt; 
		

		if ( abs(tri[0][0]) < 1e-5 && abs(tri[1][0]) < 1e-5 && abs(tri[2][0]) < 1e-5 )
		{
			if (abs(p[0]) < 1e-5)
			{
				tri[0][0] = tri [1][0] = tri [2][0] = 1;
				p[0] = 1;
			}
			else 
				return false; 
			//use is_in_triangle to calculate.
		}
		else
	    if ( abs(tri[0][1]) < 1e-5 && abs(tri[1][1]) < 1e-5 && abs(tri[2][1]) < 1e-5 )
		{
			if (abs(p[1]) < 1e-5)
			{
				tri[0][1] = tri [1][1] = tri [2][1] = 1;
				p[1] = 1;
				
			}
			else 
				return false;
		}
		else
		if ( abs(tri[0][2]) < 1e-5 && abs(tri[1][2]) < 1e-5 && abs(tri[2][2]) < 1e-5 )
		{
			if (abs(p[2])< 1e-5)
			{
				tri[0][2] = tri [1][2] = tri [2][2] = 1;
				p[2] = 1;
			}
			return false; 
		}

		MATRIX A (3,3);
		for (int i = 0; i < 3; ++i)
		{
			for (int j = 0; j < 3; ++j)
			{
				A(i,j ) = tri[i][j]; 
			}
		}
		VECTOR b(3);
		for (int i = 0; i < 3; ++i)
		{
			b[i] = p[i];
		}
		VECTOR solution;
		LU_Solver (A, b, solution);
		edge_idx = -1;
		if (abs (1 - solution[0] - solution[1] - solution[2])< 1e-5 && solution[0] >= 0 && solution[1] >=0 && solution[2]>=0)
		{
			/*
			for (int i = 0; i < 3; ++i)
			{
				if (solution[i] < 0)
				{
					edge_idx = 3 - i - 1;
					break;
				}
			}
			*/
			return true; 
		}
		else
		{
			return false;
		}
	}

	static bool is_in_triangle (const POINT tri[], const POINT & p, int & edge_idx)
	{
		VECTOR v[3];
		v[0] = point2vector (tri[0] - p);
		v[1] = point2vector (tri[1] - p);
		v[2] = point2vector (tri[2] - p);

		VECTOR cp(3);
		for (int i = 0; i < 3; ++i)
		{
			cp = cross_product(v[i], v[(i+1) % 3]);
			edge_idx = i;
			if (cp[2] < 0)
			{
				return false;
			}
		}
		return true;
	}	

	static bool interpolation3 (const POINT flip_v[], const VALUE value[], VECTOR & coeff)
	{
		double data [3][3] ;
		//a0 + a1 x + a2 y + a3 x^2 + a4 y ^2 + a5 xy
		VECTOR b(3);
		for (int i = 0; i < 3; ++i)
		{
			data[i][0] = 1;
			data[i][1] = flip_v[i][0];
			data[i][2] = flip_v[i][1];
			b[i] = value[i];
		}
		MATRIX M(data);
		return  LU_Solver (M,b, coeff);
	}

	static bool interpolation (const POINT flip_v[], const VALUE value[], VECTOR & coeff)
	{
		double data [6][6] ;
		//a0 + a1 x + a2 y + a3 x^2 + a4 y ^2 + a5 xy
		VECTOR b(6);
		for (int i = 0; i < 6; ++i)
		{
			data[i][0] = 1;
			data[i][1] = flip_v[i][0];
			data[i][2] = flip_v[i][1];
			data[i][3] = flip_v[i][0] * flip_v[i][0];
			data[i][4] = flip_v[i][1] * flip_v[i][1];
			data[i][5] = flip_v[i][0] * flip_v[i][1];
			b[i] = value[i];
		}
	
		MATRIX M(data);

		return  LU_Solver (M,b, coeff);
	}
	static VALUE f(const VALUE a[], const VALUE x, const VALUE y)
	{
		return (a[0] + a[1]* x + a[2]* y + a[3]* x*x + a[4]* y*y + a[5]* x*y);
	}
	static VALUE gradx (ARRAY coeff, const VALUE x, const VALUE y)
	{
		return( coeff[1] + 2 * coeff[3] * x + coeff[5] * y);

	}
	static VALUE grady(ARRAY coeff, const VALUE x, const VALUE y)
	{
		return ( coeff[2] + 2 * coeff[4] * y + coeff[5] * x);
	}
	inline static VECTOR cross_product (const VECTOR & v1, const VECTOR & v2)
	{
		return mtl::vector::cross(v1, v2);
	}

	//Vector & Point convension
	inline static VECTOR point2vector (const POINT & p)
	{
		VECTOR v(3);
		v[0] = p[0]; v[1] = p[1] ; v[2] = p[2];
		return v;
	}
	inline static POINT vector2point (const VECTOR & v)
	{
		POINT p;
		p[0] = v[0]; p[1] = v[1]; p[2] = v[2]; 
		return p;
	}

public:
	static void generate_seperate_mesh_geodesics(Mesh & mesh, std::list<Mesh::Point> & path)
	{
		unsigned n_vertex = mesh.n_vertices();
		std::cout<<"o model"<<std::endl;
		for (unsigned i = 0; i < n_vertex; ++i)
		{
			std::cout<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
		}
	
		std::cout<<"s off"<<std::endl;
		for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
		{
			std::cout<<"f ";
			for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
			{
				std::cout<<fv_it.handle().idx() + 1<<' ';
			}
			std::cout<<std::endl;
		}

		std::cout<<"o geodesics"<<std::endl;
		for (std::list<Mesh::Point>::iterator it = path.begin(); it!=path.end(); ++it)
		{
			std::cout<<"v "<<(*it)<<std::endl;
		}
		std::cout<<"s off"<<std::endl;
		for (int i = 0; i < int(path.size()) - 1;++i)
		{	
			std::cout<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
		}
	
	}
};

void generate_seperate_mesh_geodesics(Mesh & mesh, std::list<Mesh::Point> & path, std::ostream & ostr); 

void generate_seperate_group_geodesics(Mesh & mesh, std::list<std::list<Mesh::Point> > & path_groups, std::ostream & ostr);
#endif
